#include <bits/stdint-intn.h>
#include <stdio.h>
#include "mg_typedef.h"
#include "obstacle_avoidance_sonar.h"
#include "message_loop/message_loop.h"
#if defined( __linux__ )
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/udp.h>
#elif defined( __VXWORKS__ )
#endif

static struct SonarAttr {
  int32_t            socket_fd_;
  struct sockaddr_in sock_addr_;
  struct ev_loop*    loop_;
} g_sonar_attr;
static void sonar_status_message_handler( EV_P_ ev_io* w, int revents ) {
  int32_t            lengh = 0;
  struct sockaddr_in addr;
  socklen_t          addr_len = sizeof( addr );
  SonarStatusCommand buf;
  lengh = recvfrom( w->fd,
                    &buf,
                    sizeof( buf ),
                    0,
                    ( struct sockaddr* )&addr,
                    &addr_len );

  MG_DEBUG( "recved data length=%d", lengh );
}
void sonar_init( ) {
  int32_t            socket_fd = -1;
  struct sockaddr_in sock_addr = { 0 };
  int32_t            ret       = -1;

  sock_addr.sin_family         = AF_INET;
  sock_addr.sin_addr.s_addr    = INADDR_ANY;
  sock_addr.sin_port           = htons( 2222 );

  //
  socket_fd = socket( AF_INET, SOCK_DGRAM, IPPROTO_UDP );

  ret = bind( socket_fd, ( struct sockaddr* )&sock_addr, sizeof( sock_addr ) );

  {
    static ev_io    sonar_io_watcher;
    struct ev_loop* loop = ev_default_loop( 0 );
    ev_io_init( &sonar_io_watcher,
                sonar_status_message_handler,
                socket_fd,
                EV_READ );
    g_sonar_attr.loop_    = loop;
    sonar_io_watcher.data = &g_sonar_attr;

    ev_io_start( loop, &sonar_io_watcher );
  }
}